using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.DirectX;
using Microsoft.DirectX.Direct3D;

namespace TgcViewer.Utils.Input
{
    /// <summary>
    /// Camara en primera persona
    /// </summary>
    public class TgcFpsCamera
    {
        /// <summary>
        /// Comportamiento de camara
        /// </summary>
        public enum CameraBehavior
        {
            CAMERA_BEHAVIOR_FIRST_PERSON,
            CAMERA_BEHAVIOR_FLIGHT,
        };

        const float DEFAULT_ROTATION_SPEED = 1f;
        const float CAMERA_SPEED_FLIGHT_YAW = 100.0f;
        readonly Vector3 CAMERA_VELOCITY = new Vector3(2.0f, 2.0f, 2.0f);
        readonly Vector3 CAMERA_POS = new Vector3(0.0f, 1.0f, 0.0f);
        readonly Vector3 CAMERA_ACCELERATION = new Vector3(8.0f, 8.0f, 8.0f);

        readonly Vector3 WORLD_XAXIS = new Vector3(1.0f, 0.0f, 0.0f);
        readonly Vector3 WORLD_YAXIS = new Vector3(0.0f, 1.0f, 0.0f);
        readonly Vector3 WORLD_ZAXIS = new Vector3(0.0f, 0.0f, 1.0f);
        readonly Vector3 DEFAULT_UP_VECTOR = new Vector3(0.0f, 1.0f, 0.0f);


        float accumPitchDegrees;
        Vector3 eye;
        Vector3 xAxis;
        Vector3 yAxis;
        Vector3 zAxis;
        Vector3 viewDir;

        #region Getters y Setters

        Vector3 acceleration;
        /// <summary>
        /// Aceleracion de la camara
        /// </summary>
        public Vector3 Acceleration
        {
            get { return acceleration; }
            set { acceleration = value; }
        }

        CameraBehavior behavior;
        /// <summary>
        /// Configurar comportamiento de camara
        /// </summary>
        public CameraBehavior Behavior
        {
            get { return behavior; }
            set
            {
                if (behavior == CameraBehavior.CAMERA_BEHAVIOR_FLIGHT && value == CameraBehavior.CAMERA_BEHAVIOR_FIRST_PERSON)
                {
                    // Moving from flight behavior to first person behavior.
                    // Need to ignore camera roll, but retain existing pitch and heading.
                    lookAt(eye, eye + zAxis, WORLD_YAXIS);
                }
                behavior = value;
            }
        }

        Vector3 currentVelocity;
        /// <summary>
        /// Velocidad de desplazamiento actual
        /// </summary>
        public Vector3 CurrentVelocity
        {
            get { return currentVelocity; }
            set { currentVelocity = value; }
        }

        Vector3 velocity;
        /// <summary>
        /// Velocidad de desplazamiento
        /// </summary>
        public Vector3 Velocity
        {
            get { return velocity; }
            set { velocity = value; }
        }

        float rotationSpeed;
        /// <summary>
        /// Velocidad de rotacion
        /// </summary>
        public float RotationSpeed
        {
            get { return rotationSpeed; }
            set { rotationSpeed = value; }
        }

        Matrix viewMatrix;
        /// <summary>
        /// View Matrix resultante
        /// </summary>
        public Matrix ViewMatrix
        {
            get { return viewMatrix; }
        }

        #endregion

        public TgcFpsCamera()
        {
            behavior = CameraBehavior.CAMERA_BEHAVIOR_FIRST_PERSON;
            accumPitchDegrees = 0.0f;
            rotationSpeed = DEFAULT_ROTATION_SPEED;
            eye = new Vector3(0.0f, 0.0f, 0.0f);
            xAxis = new Vector3(1.0f, 0.0f, 0.0f);
            yAxis = new Vector3(0.0f, 1.0f, 0.0f);
            zAxis = new Vector3(0.0f, 0.0f, 1.0f);
            viewDir = new Vector3(0.0f, 0.0f, 1.0f);
            accumPitchDegrees = 0;

            acceleration = CAMERA_ACCELERATION;
            currentVelocity = new Vector3(0.0f, 0.0f, 0.0f);
            velocity = CAMERA_VELOCITY;
            acceleration = CAMERA_ACCELERATION;
            viewMatrix = Matrix.Identity;
            setPosition(CAMERA_POS);
        }

        public void lookAt(Vector3 eye, Vector3 target, Vector3 up)
        {
            this.eye = eye;

            zAxis = target - eye;
            zAxis.Normalize();

            viewDir = zAxis;

            xAxis = Vector3.Cross(up, zAxis);
            xAxis.Normalize();

            yAxis = Vector3.Cross(zAxis, xAxis);
            yAxis.Normalize();
            //xAxis.Normalize();

            viewMatrix = Matrix.Identity;

            viewMatrix.M11 = xAxis.X;
            viewMatrix.M21 = xAxis.Y;
            viewMatrix.M31 = xAxis.Z;
            viewMatrix.M41 = -Vector3.Dot(xAxis, eye);

            viewMatrix.M12 = yAxis.X;
            viewMatrix.M22 = yAxis.Y;
            viewMatrix.M32 = yAxis.Z;
            viewMatrix.M42 = -Vector3.Dot(yAxis, eye);

            viewMatrix.M13 = zAxis.X;
            viewMatrix.M23 = zAxis.Y;
            viewMatrix.M33 = zAxis.Z;
            viewMatrix.M43 = -Vector3.Dot(zAxis, eye);

            // Extract the pitch angle from the view matrix.
            accumPitchDegrees = Geometry.RadianToDegree((float)-Math.Asin((double)viewMatrix.M23));
        }

        public void lookAt(Vector3 eye, Vector3 target)
        {
            lookAt(eye, target, DEFAULT_UP_VECTOR);
        }

        /// <summary>
        /// Moves the camera by dx world units to the left or right; dy
        /// world units upwards or downwards; and dz world units forwards
        /// or backwards.
        /// </summary>
        public void move(float dx, float dy, float dz)
        {

            Vector3 auxEye = this.eye;
            Vector3 forwards;

            if (behavior == CameraBehavior.CAMERA_BEHAVIOR_FIRST_PERSON)
            {
                // Calculate the forwards direction. Can't just use the camera's local
                // z axis as doing so will cause the camera to move more slowly as the
                // camera's view approaches 90 degrees straight up and down.
                forwards = Vector3.Cross(xAxis, WORLD_YAXIS);
                forwards.Normalize();
            }
            else
            {
                forwards = viewDir;
            }

            auxEye += xAxis * dx;
            auxEye += WORLD_YAXIS * dy;
            auxEye += forwards * dz;

            setPosition(auxEye);
        }

        /// <summary>
        /// Moves the camera by the specified amount of world units in the specified
        /// direction in world space. 
        /// </summary>
        public void move(Vector3 direction, Vector3 amount)
        {
            eye.X += direction.X * amount.X;
            eye.Y += direction.Y * amount.Y;
            eye.Z += direction.Z * amount.Z;

            updateViewMatrix(false);
        }

        /// <summary>
        /// Rotates the camera based on its current behavior.
        /// Note that not all behaviors support rolling.
        ///
        /// This Camera class follows the left-hand rotation rule.
        /// Angles are measured clockwise when looking along the rotation
        /// axis toward the origin. Since the Z axis is pointing into the
        /// screen we need to negate rolls.
        /// </summary>
        public void rotate(float headingDegrees, float pitchDegrees, float rollDegrees)
        {
            rollDegrees = -rollDegrees;

            switch (behavior)
            {
            default:
                break;

            case CameraBehavior.CAMERA_BEHAVIOR_FIRST_PERSON:
                rotateFirstPerson(headingDegrees, pitchDegrees);
                break;

            case CameraBehavior.CAMERA_BEHAVIOR_FLIGHT:
                rotateFlight(headingDegrees, pitchDegrees, rollDegrees);
                break;
            }

            updateViewMatrix(true);
        }

        /// <summary>
        /// This method applies a scaling factor to the rotation angles prior to
        /// using these rotation angles to rotate the camera. This method is usually
        /// called when the camera is being rotated using an input device (such as a
        /// mouse or a joystick).
        /// </summary>
        public void rotateSmoothly(float headingDegrees, float pitchDegrees, float rollDegrees)
        {
            headingDegrees *= rotationSpeed;
            pitchDegrees *= rotationSpeed;
            rollDegrees *= rotationSpeed;

            rotate(headingDegrees, pitchDegrees, rollDegrees);
        }

        /// <summary>
        /// Moves the camera using Newton's second law of motion. Unit mass is
        /// assumed here to somewhat simplify the calculations. The direction vector
        /// is in the range [-1,1].
        /// </summary>
        public void updatePosition(Vector3 direction, float elapsedTimeSec)
        {
            if(Vector3.LengthSq(currentVelocity) != 0.0f)
            {
                // Only move the camera if the velocity vector is not of zero length.
                // Doing this guards against the camera slowly creeping around due to
                // floating point rounding errors.

                Vector3 displacement = (currentVelocity * elapsedTimeSec) +
                    (0.5f * acceleration * elapsedTimeSec * elapsedTimeSec);

                // Floating point rounding errors will slowly accumulate and cause the
                // camera to move along each axis. To prevent any unintended movement
                // the displacement vector is clamped to zero for each direction that
                // the camera isn't moving in. Note that the updateVelocity() method
                // will slowly decelerate the camera's velocity back to a stationary
                // state when the camera is no longer moving along that direction. To
                // account for this the camera's current velocity is also checked.

                if (direction.X == 0.0f && Math.Abs(currentVelocity.X) < 1e-6f)
                    displacement.X = 0.0f;

                if (direction.Y == 0.0f && Math.Abs(currentVelocity.Y) < 1e-6f)
                    displacement.Y = 0.0f;

                if (direction.Z == 0.0f && Math.Abs(currentVelocity.Z) < 1e-6f)
                    displacement.Z = 0.0f;

                move(displacement.X, displacement.Y, displacement.Z);
            }

            // Continuously update the camera's velocity vector even if the camera
            // hasn't moved during this call. When the camera is no longer being moved
            // the camera is decelerating back to its stationary state.

            updateVelocity(direction, elapsedTimeSec);
        }

        public void setPosition(Vector3 pos)
        {
            eye = pos;

            updateViewMatrix(false);
        }

        public void rotateFirstPerson(float headingDegrees, float pitchDegrees)
        {
            accumPitchDegrees += pitchDegrees;

            if (accumPitchDegrees > 90.0f)
            {
                pitchDegrees = 90.0f - (accumPitchDegrees - pitchDegrees);
                accumPitchDegrees = 90.0f;
            }

            if (accumPitchDegrees < -90.0f)
            {
                pitchDegrees = -90.0f - (accumPitchDegrees - pitchDegrees);
                accumPitchDegrees = -90.0f;
            }

            float heading = Geometry.DegreeToRadian(headingDegrees);
            float pitch = Geometry.DegreeToRadian(pitchDegrees);
            
            Matrix rotMtx;
            Vector4 result;

            // Rotate camera's existing x and z axes about the world y axis.
            if (heading != 0.0f)
            {
                rotMtx = Matrix.RotationY(heading);

                result = Vector3.Transform(xAxis, rotMtx);
                xAxis = new Vector3(result.X, result.Y, result.Z);

                result = Vector3.Transform(zAxis, rotMtx);
                zAxis = new Vector3(result.X, result.Y, result.Z);
            }

            // Rotate camera's existing y and z axes about its existing x axis.
            if (pitch != 0.0f)
            {
                rotMtx = Matrix.RotationAxis(xAxis, pitch);

                result = Vector3.Transform(yAxis, rotMtx);
                yAxis = new Vector3(result.X, result.Y, result.Z);

                result = Vector3.Transform(zAxis, rotMtx);
                zAxis = new Vector3(result.X, result.Y, result.Z);
            }
        }

        public void rotateFlight(float headingDegrees, float pitchDegrees, float rollDegrees)
        {
            float heading = Geometry.DegreeToRadian(headingDegrees);
            float pitch = Geometry.DegreeToRadian(pitchDegrees);
            float roll = Geometry.DegreeToRadian(rollDegrees);

            Matrix rotMtx;
            Vector4 result;

            // Rotate camera's existing x and z axes about its existing y axis.
            if (heading != 0.0f)
            {
                rotMtx = Matrix.RotationAxis(yAxis, heading);

                result = Vector3.Transform(xAxis, rotMtx);
                xAxis = new Vector3(result.X, result.Y, result.Z);

                result = Vector3.Transform(zAxis, rotMtx);
                zAxis = new Vector3(result.X, result.Y, result.Z);
            }

            // Rotate camera's existing y and z axes about its existing x axis.
            if (pitch != 0.0f)
            {
                rotMtx = Matrix.RotationAxis(xAxis, heading);

                result = Vector3.Transform(yAxis, rotMtx);
                yAxis = new Vector3(result.X, result.Y, result.Z);

                result = Vector3.Transform(zAxis, rotMtx);
                zAxis = new Vector3(result.X, result.Y, result.Z);
            }

            // Rotate camera's existing x and y axes about its existing z axis.
            if (roll != 0.0f)
            {
                rotMtx = Matrix.RotationAxis(zAxis, heading);

                result = Vector3.Transform(xAxis, rotMtx);
                xAxis = new Vector3(result.X, result.Y, result.Z);

                result = Vector3.Transform(yAxis, rotMtx);
                yAxis = new Vector3(result.X, result.Y, result.Z);
            }
        }

        /// <summary>
        /// Updates the camera's velocity based on the supplied movement direction
        /// and the elapsed time (since this method was last called). The movement
        /// direction is the in the range [-1,1].
        /// </summary>
        public void updateVelocity(Vector3 direction, float elapsedTimeSec)
        {
            if (direction.X != 0.0f)
            {
                // Camera is moving along the x axis.
                // Linearly accelerate up to the camera's max speed.

                currentVelocity.X += direction.X * acceleration.X * elapsedTimeSec;

                if (currentVelocity.X > velocity.X)
                    currentVelocity.X = velocity.X;
                else if (currentVelocity.X < -velocity.X)
                    currentVelocity.X = -velocity.X;
            }
            else
            {
                // Camera is no longer moving along the x axis.
                // Linearly decelerate back to stationary state.

                if (currentVelocity.X > 0.0f)
                {
                    if ((currentVelocity.X -= acceleration.X * elapsedTimeSec) < 0.0f)
                        currentVelocity.X = 0.0f;
                }
                else
                {
                    if ((currentVelocity.X += acceleration.X * elapsedTimeSec) > 0.0f)
                        currentVelocity.X = 0.0f;
                }
            }

            if (direction.Y != 0.0f)
            {
                // Camera is moving along the y axis.
                // Linearly accelerate up to the camera's max speed.

                currentVelocity.Y += direction.Y * acceleration.Y * elapsedTimeSec;

                if (currentVelocity.Y > velocity.Y)
                    currentVelocity.Y = velocity.Y;
                else if (currentVelocity.Y < -velocity.Y)
                    currentVelocity.Y = -velocity.Y;
            }
            else
            {
                // Camera is no longer moving along the y axis.
                // Linearly decelerate back to stationary state.

                if (currentVelocity.Y > 0.0f)
                {
                    if ((currentVelocity.Y -= acceleration.Y * elapsedTimeSec) < 0.0f)
                        currentVelocity.Y = 0.0f;
                }
                else
                {
                    if ((currentVelocity.Y += acceleration.Y * elapsedTimeSec) > 0.0f)
                        currentVelocity.Y = 0.0f;
                }
            }

            if (direction.Z != 0.0f)
            {
                // Camera is moving along the z axis.
                // Linearly accelerate up to the camera's max speed.

                currentVelocity.Z += direction.Z * acceleration.Z * elapsedTimeSec;

                if (currentVelocity.Z > velocity.Z)
                    currentVelocity.Z = velocity.Z;
                else if (currentVelocity.Z < -velocity.Z)
                    currentVelocity.Z = -velocity.Z;
            }
            else
            {
                // Camera is no longer moving along the z axis.
                // Linearly decelerate back to stationary state.

                if (currentVelocity.Z > 0.0f)
                {
                    if ((currentVelocity.Z -= acceleration.Z * elapsedTimeSec) < 0.0f)
                        currentVelocity.Z = 0.0f;
                }
                else
                {
                    if ((currentVelocity.Z += acceleration.Z * elapsedTimeSec) > 0.0f)
                        currentVelocity.Z = 0.0f;
                }
            }
        }

        public void updateViewMatrix(bool orthogonalizeAxes)
        {
            if (orthogonalizeAxes)
            {
                // Regenerate the camera's local axes to orthogonalize them.

                zAxis.Normalize();

                yAxis = Vector3.Cross(zAxis, xAxis);
                yAxis.Normalize();

                xAxis = Vector3.Cross(yAxis, zAxis);
                xAxis.Normalize();

                viewDir = zAxis;
            }

            // Reconstruct the view matrix.

            viewMatrix.M11 = xAxis.X;
            viewMatrix.M21 = xAxis.Y;
            viewMatrix.M31 = xAxis.Z;
            viewMatrix.M41 = -Vector3.Dot(xAxis, eye);

            viewMatrix.M12 = yAxis.X;
            viewMatrix.M22 = yAxis.Y;
            viewMatrix.M32 = yAxis.Z;
            viewMatrix.M42 = -Vector3.Dot(yAxis, eye);

            viewMatrix.M13 = zAxis.X;
            viewMatrix.M23 = zAxis.Y;
            viewMatrix.M33 = zAxis.Z;
            viewMatrix.M43 = -Vector3.Dot(zAxis, eye);

            viewMatrix.M14 = 0.0f;
            viewMatrix.M24 = 0.0f;
            viewMatrix.M34 = 0.0f;
            viewMatrix.M44 = 1.0f;
        }

        public void updateCamera(float elapsedTimeSec, TgcD3dInput d3dInput)
        {
            float heading = 0.0f;
            float pitch = 0.0f;
            float roll = 0.0f;

            Vector3 direction = getMovementDirection(d3dInput);

            switch (behavior)
            {
            case CameraBehavior.CAMERA_BEHAVIOR_FIRST_PERSON:
                pitch = d3dInput.YposRelative * rotationSpeed;
                heading = d3dInput.XposRelative * rotationSpeed;

                rotate(heading, pitch, 0.0f);
                break;

            case CameraBehavior.CAMERA_BEHAVIOR_FLIGHT:
                heading = direction.X * CAMERA_SPEED_FLIGHT_YAW * elapsedTimeSec;
                pitch = -d3dInput.YposRelative * rotationSpeed;
                roll = d3dInput.XposRelative * rotationSpeed;

                rotate(heading, pitch, roll);
                direction.X = 0.0f; // ignore yaw motion when updating camera velocity
                break;
            }

            updatePosition(direction, elapsedTimeSec);

            //TODO callback para deteccion de colisiones
        }

        bool moveForwardsPressed = false;
        bool moveBackwardsPressed = false;
        bool moveRightPressed = false;
        bool moveLeftPressed = false;
        bool moveUpPressed = false;
        bool moveDownPressed = false;

        public Vector3 getMovementDirection(TgcD3dInput d3dInput)
        {
            Vector3 direction = new Vector3(0.0f, 0.0f, 0.0f);

            if (d3dInput.keyDown(Microsoft.DirectX.DirectInput.Key.Up) || d3dInput.keyDown(Microsoft.DirectX.DirectInput.Key.W))
            {
                if (!moveForwardsPressed)
                {
                    moveForwardsPressed = true;
                    currentVelocity = new Vector3(currentVelocity.X, currentVelocity.Y, 0.0f);
                }

                direction.Z += 1.0f;
            }
            else
            {
                moveForwardsPressed = false;
            }

            if (d3dInput.keyDown(Microsoft.DirectX.DirectInput.Key.Down) || d3dInput.keyDown(Microsoft.DirectX.DirectInput.Key.S))
            {
                if (!moveBackwardsPressed)
                {
                    moveBackwardsPressed = true;
                    currentVelocity = new Vector3(currentVelocity.X, currentVelocity.Y, 0.0f);
                }

                direction.Z -= 1.0f;
            }
            else
            {
                moveBackwardsPressed = false;
            }

            if (d3dInput.keyDown(Microsoft.DirectX.DirectInput.Key.Right) || d3dInput.keyDown(Microsoft.DirectX.DirectInput.Key.D))
            {
                if (!moveRightPressed)
                {
                    moveRightPressed = true;
                    currentVelocity = new Vector3(0.0f, currentVelocity.Y, currentVelocity.Z);
                }

                direction.X += 1.0f;
            }
            else
            {
                moveRightPressed = false;
            }

            if (d3dInput.keyDown(Microsoft.DirectX.DirectInput.Key.Left) || d3dInput.keyDown(Microsoft.DirectX.DirectInput.Key.A))
            {
                if (!moveLeftPressed)
                {
                    moveLeftPressed = true;
                    currentVelocity = new Vector3(0.0f, currentVelocity.Y, currentVelocity.Z);
                }

                direction.X -= 1.0f;
            }
            else
            {
                moveLeftPressed = false;
            }

            if (d3dInput.keyDown(Microsoft.DirectX.DirectInput.Key.Space))
            {
                if (!moveUpPressed)
                {
                    moveUpPressed = true;
                    currentVelocity = new Vector3(currentVelocity.X, 0.0f, currentVelocity.Z);
                }

                direction.Y += 1.0f;
            }
            else
            {
                moveUpPressed = false;
            }

            if (d3dInput.keyDown(Microsoft.DirectX.DirectInput.Key.LeftControl))
            {
                if (!moveDownPressed)
                {
                    moveDownPressed = true;
                    currentVelocity = new Vector3(currentVelocity.X, 0.0f, currentVelocity.Z);
                }

                direction.Y -= 1.0f;
            }
            else
            {
                moveDownPressed = false;
            }

            return direction;
        }




    }
}
